#include <stdio.h>
#include <unistd.h>
#include <math.h>
#include "xparameters.h"
#include "xtime_l.h"
#include "xil_cache.h"

#include "xuwb_factor.h"
#include "xuwb_variable.h"
#include "xbram.h"

#include "env_config.h"
#include "helper_functions.h"

#define F_NUM XPAR_XUWB_FACTOR_NUM_INSTANCES
#define V_NUM XPAR_XUWB_VARIABLE_NUM_INSTANCES
#define B_NUM XPAR_XBRAM_NUM_INSTANCES

#define CHKPOINT 25
#define THRHOD 1e-4
#define true 1u
#define false 0u

struct Measures *mea;
XUwb_factor factors[F_NUM] = {};
XUwb_variable variables[V_NUM] = {};
XBram brams[B_NUM] = {};

int Run_on_ARM(struct Pmf *pmf_out);
int Run_on_FPGA(struct Pmf *pmf_out);

int main(){
	printf("hello world!\n\r");

	int Status;
	struct Pmf pmf_grid={};
	struct Pmf* pmf_std=NULL;
	float err = 0;
	u8 flag = 1;
	float std_value, grid_value;

	/* Init all IPs */
	XTime_rst();
	XTime_mask(0);
	Status = Factors_Init(factors, F_NUM);
	if(Status != XST_SUCCESS){
		printf("Factor initialize failed!\n\r");
		return 0;
	}
	Status = Variables_Init(variables, V_NUM);
	if(Status != XST_SUCCESS){
		printf("Variable initialize failed!\n\r");
		return 0;
	}
	Status = Brams_Init(brams, B_NUM);
	if(Status != XST_SUCCESS){
		printf("Bram Controller initialize failed!\n\r");
		return 0;
	}
	XTime_trigger(0,0);
	printf("Initial cost %lu\n\r", tUsed[0]);

	// Xil_DCacheDisable();
	/* Start test */
	for(int i = 0; i < CHKPOINT; i++){
		printf("point %d start! >==============\n\r", i);
		mea = &measurements[i];
		// start run a test point
		printf("Launching ARM ---------\n\r");
		Run_on_ARM(&pmf_grid);
		printf("Launching FPGA---------\n\r");
		Run_on_FPGA(&pmf_grid);
		// finish run
		flag = true;
		pmf_std = &pmfs[i];
		for(int k = 0; k < CARD*CARD; k++){
			std_value = pmf_std->pmf[k/CARD][k%CARD];
			grid_value = pmf_grid.pmf[k/CARD][k%CARD];
			err = std_value - grid_value;
			if(fabs(err) >= THRHOD){
				flag = false;
				printf("TB: point%d[%d][%d] failed! %.8f(%.8f), std = %.6f\n\r",
						i, k/CARD, k%CARD, grid_value, fabs(err), std_value);
			}
		}
		if (flag)
			printf("================> point%d pass!\n\n\r", i);
		else
			printf("================> point%d fail!\n\n\r", i);
	};


    return 0;
}

int Run_on_FPGA(struct Pmf *pmf_out){
	data_t buf;

	XTime_rst();
	XTime_mask(0);	// total start
	/* launch&wait Factors */
	XTime_mask(1);	// factors start
	for (int j = 0; j < F_NUM; j++)
	{
		buf.f = mea->s[j];
		while (!XUwb_factor_IsReady(&factors[j])){};
		XUwb_factor_Set_z(&factors[j], buf.u);
		XUwb_factor_Start(&factors[j]);
	}
	XTime_trigger(1,1);	// factors function

	for(int j = 0; j < F_NUM; j++){
		while(!XUwb_factor_IsDone(&factors[j])){};
	}
	XTime_trigger(1,2);	// factors all_done

	/* launch&wait Variables */
	XTime_mask(2);	// variables start
	for (int j = 0; j < V_NUM; j++)
	{
		while (!XUwb_variable_IsReady(&variables[j])){};
		XUwb_variable_Start(&variables[j]);
	}
	XTime_trigger(2,3);	// variables functions

	for (int j = 0; j < V_NUM;  j++){
		while (!XUwb_variable_IsDone(&variables[j])){};
	}
	XTime_trigger(2,4);	// variables all_done

	/* Test bram RW */
	XTime_mask(3);	// copy start
	Bram_Read_helper(&brams[0], pmf_out, CARD * CARD);
	XTime_trigger(3,5);	// copy done
	XTime_trigger(0, 0);	// total done

	// printf("single factor functions = %lu\n\r", tUsed[1]);
	// printf("single factor all_done = %lu\n\r", tUsed[2]);
	// printf("single variable functions = %lu\n\r", tUsed[3]);
	// printf("single variable all_done = %lu\n\r", tUsed[4]);
	// printf("data load done = %lu\n\r", tUsed[5]);
	// printf("total FPGA funciton = %lu\n\r", tUsed[0]);
	// printf("factors(launch)\tvariable(launch)\tdata\ttotal\n\r");
	printf("FPGA,%lu(%lu),%lu(%lu),%lu,%lu\n\r", tUsed[2], tUsed[1], tUsed[4], tUsed[3], tUsed[5], tUsed[0]);
	return XST_SUCCESS;
}

int Run_on_ARM(struct Pmf *pmf_out){
	struct Pmf msg_pmf[F_NUM] = {};

	XTime_rst();
	XTime_mask(0);
	XTime_mask(1);
	for(int i = 0; i < F_NUM; i++){
		UWB_Factor(stations[i], mea->s, x_range, y_range, msg_pmf[i].pmf);
		// printf("single Factor funciton = %lu\n\r", tUsed);
	}
	XTime_trigger(1,1);
	
	XTime_mask(2);
	for(int i = 0; i < V_NUM; i++){
		UWB_Variable(pmf_out->pmf, msg_pmf[0].pmf, msg_pmf[1].pmf, msg_pmf[2].pmf, msg_pmf[3].pmf);
		// printf("single Variable funciton = %lu\n\r", tUsed);
	}
	XTime_trigger(2,2);
	XTime_trigger(0,0);
	// printf("single factor all_done = %lu\n\r", tUsed[1]);
	// printf("single variable all_done = %lu\n\r", tUsed[2]);
	// printf("total ARM funciton = %lu\n\r", tUsed[0]);
	// printf("factors\tvariables\ttotal\n\r");
	printf("ARM,%lu,%lu,%lu\n\r", tUsed[1], tUsed[2], tUsed[0]);
	return XST_SUCCESS;
}
